#include "main.h"

uint16_t xdata task1, task2;
unsigned int g_PCA_Capture_us[3] = {0};

void delay_ms(u16 ms)
{
	unsigned char i, j;
		while(ms--){
		_nop_();
		_nop_();
		i = 44;
		j = 8;
		do
		{
			while (--j);
		} while (--i);
	}
}

void my_memset(char *p, char d, int len)
{
	while (len--)
	{
		*p = d;
		p++;
	}
}

void get_pca_capture_us(void)
{
	char i = 0;
	for (i = 0; i < 3; i++) {
		if (g_PCA_Capture[i]) {
			g_PCA_Capture_us[i] = (long)g_PCA_Capture[i] * 625 / 1728;
			if (g_PCA_Capture_us[i] < 1000) {
				g_PCA_Capture_us[i] = 1000;
			}
			if (g_PCA_Capture_us[i] > 2000) {
				g_PCA_Capture_us[i] = 2000;
			}
		}
		else
			g_PCA_Capture_us[i] = 0;
	}
	my_memset((char*)g_PCA_Capture, 0, sizeof(g_PCA_Capture));
}


void main()
{
	unsigned int tmp = 0;
	char i = 0;
	P0M0 = 0x00;
	P0M1 = 0x00;
	P1M0 = 0x00;
	P1M1 = 0x00;
	P2M0 = 0x00;
	P2M1 = 0x00;
	P3M0 = 0x00;
	P3M1 = 0x00;
	P4M0 = 0x00;
	P4M1 = 0x00;
	P5M0 = 0x00;
	P5M1 = 0x00;

	P3M1 = 0x00; P3M0 = 0x7C; //P3(双推推推推推双双)

	LED = 0; 
	M1A = 0; 
	M1B = 0; 
	M2A = 0; 
	M2B = 0; 

	Uart_init();
	Timer_Init();
	pca_init();
	delay_ms(1);
	EA = 1;
	
	printf("car driver debug\r\n");
	printf("time:%s\r\n",__TIME__);

	while (1)
	{
		if(task1 >= 50){
			task1 = 0;
			get_pca_capture_us();
			tmp = 0;
			for (i = 0; i < 2; i++) {
				if (g_PCA_Capture_us[i] >= 1510) {
					duty_polarity[i] = 1;
					tmp = (g_PCA_Capture_us[i] - 1510 + 5) / 5;
				} else if (g_PCA_Capture_us[i] >= 1000 && g_PCA_Capture_us[i] <= 1490) {
					duty_polarity[i] = 0;
					tmp = (1490 - g_PCA_Capture_us[i] + 5) / 5;
				} else {
					tmp = 0;
				}

				duty_target[i] =  tmp;
			}

			if (g_PCA_Capture_us[2] > 1000) {
				duty_target[2] =  (g_PCA_Capture_us[2] - 1000 + 5) / 10;
			} else {
				duty_target[2] = 0;
			}

		}

		if(task2 >= 100){
			task2 = 0;
			printf("CH1:%uus duty:%u\n", g_PCA_Capture_us[0], (int)duty_target[0]);
			printf("CH2:%uus duty:%u\n", g_PCA_Capture_us[1], (int)duty_target[1]);
			printf("CH3:%uus duty:%u\n", g_PCA_Capture_us[2], (int)duty_target[2]);
		}

		if(rx1_sta & 0x80)Uart1_handle();
	}
}



